graph_search.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
#include "core/common/wolf.h"
#include "core/frame/frame_base.h"
#include "core/factor/factor_base.h"
 
#include <map>
 
namespace wolf
{
class GraphSearch
{
  private:
    std::map<NodeStateBlocksPtr, std::pair<FactorBasePtr, NodeStateBlocksPtr>> parents_;
 
  public:
    GraphSearch();
 
    ~GraphSearch();
 
    FactorBasePtrList computeShortestPath(NodeStateBlocksPtr node1,
                                          NodeStateBlocksPtr node2,
                                          const unsigned int max_graph_dist = 0);
 
    std::set<NodeStateBlocksPtr> getNeighborFrames(const std::set<NodeStateBlocksPtr>& nodes);
 
    static FactorBasePtrList shortestPath(NodeStateBlocksPtr node1,
                                          NodeStateBlocksPtr node2,
                                          const unsigned int max_graph_dist = 0)
    {
        GraphSearch graph_search;
 
        return graph_search.computeShortestPath(node1, node2, max_graph_dist);
    }
};
 
}  // namespace wolf